#include "mpu6050.h"

/**
 * 初始化MPU6050
 * @return 0->成功;其它:失败
 */
uint8_t MPU_Init(void) {
    uint8_t res;
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X80);    //复位MPU6050
    delay_ms(100);
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X00);    //唤醒MPU6050
    MPU_Set_Gyro_Fsr(3);                    //陀螺仪传感器,±2000dps
    MPU_Set_Accel_Fsr(0);                    //加速度传感器,±2g
    MPU_Set_Rate(50);                        //设置采样率50Hz
    MPU_Write_Byte(MPU_INT_EN_REG, 0X00);    //关闭所有中断
    MPU_Write_Byte(MPU_USER_CTRL_REG, 0X00);    //I2C主模式关闭
    MPU_Write_Byte(MPU_FIFO_EN_REG, 0X00);    //关闭FIFO
    MPU_Write_Byte(MPU_INTBP_CFG_REG, 0X80);    //INT引脚低电平有效
    res = MPU_Read_Byte(MPU_DEVICE_ID_REG);
    if (res == MPU_ADDR)//器件ID正确
    {
        MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X01);    //设置CLKSEL,PLL X轴为参考
        MPU_Write_Byte(MPU_PWR_MGMT2_REG, 0X00);    //加速度与陀螺仪都工作
        MPU_Set_Rate(50);                        //设置采样率为50Hz
    } else return 1;
    return 0;
}

/**
 * 陀螺仪传感器测量范围
 * @param fsr 0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
 * @return 0->成功;其它:失败
 */
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr) {
    return MPU_Write_Byte(MPU_GYRO_CFG_REG, fsr << 3);//设置陀螺仪满量程范围
}

/**
 * 设置加速度传感器范围
 * @param fsr 0,±2g;1,±4g;2,±8g;3,±16g
 * @return 0->成功;其它:失败
 */
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr) {
    return MPU_Write_Byte(MPU_ACCEL_CFG_REG, fsr << 3);//设置加速度传感器满量程范围
}

/**
 * 设置数字低通滤波器频率
 * @param lpf 低通滤波器频率(HZ)
 * @return 0->成功;其它:失败
 */
uint8_t MPU_Set_LPF(uint16_t lpf) {
    uint8_t data = 0;
    if (lpf >= 188)data = 1;
    else if (lpf >= 98)data = 2;
    else if (lpf >= 42)data = 3;
    else if (lpf >= 20)data = 4;
    else if (lpf >= 10)data = 5;
    else data = 6;
    return MPU_Write_Byte(MPU_CFG_REG, data);//设置数字低通滤波器
}

/**
 * 设置MPU6050采样率(假定Fs=1KHz)
 * @param rate 4-1000(Hz)
 * @return 0->成功;其它:失败
 */
uint8_t MPU_Set_Rate(uint16_t rate) {
    uint8_t data;
    if (rate > 1000)rate = 1000;
    if (rate < 4)rate = 4;
    data = 1000 / rate - 1;
    data = MPU_Write_Byte(MPU_SAMPLE_RATE_REG, data);    //设置数字低通滤波器
    return MPU_Set_LPF(rate / 2);    //自动设置LPF为采样率的一半
}

/**
 * 获取温度值
 * @return 温度值
 */
float MPU_Get_Temperature(void) {
    uint8_t buf[2];
    short raw;
    float temp;
    MPU_Read_Len(MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf);
    raw = ((uint16_t) buf[0] << 8) | buf[1];
    temp = 36.53 + ((double) raw) / 340;
    return temp;
}


/**
 * 获取陀螺仪原始数据
 * @note 这个数据是带符号的
 * @param gx x方向的速度
 * @param gy y方向的速度
 * @param gz z方向的速度
 * @return 0->成功;其它:失败
 */
uint8_t MPU_Get_Gyroscope(short *gx, short *gy, short *gz) {
    uint8_t buf[6], res;
    res = MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf);
    if (res == 0) {
        *gx = ((uint16_t) buf[0] << 8) | buf[1];
        *gy = ((uint16_t) buf[2] << 8) | buf[3];
        *gz = ((uint16_t) buf[4] << 8) | buf[5];
    }
    return res;;
}

/**
 * 获取加速度的原始数据
 * @note 有符号数据
 * @param ax x方向加速度
 * @param ay y方向加速度
 * @param az z方向加速度
 * @return 0->成功;其它:失败
 */
uint8_t MPU_Get_Accelerometer(short *ax, short *ay, short *az) {
    uint8_t buf[6], res;
    res = MPU_Read_Len(MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf);
    if (res == 0) {
        *ax = ((uint16_t) buf[0] << 8) | buf[1];
        *ay = ((uint16_t) buf[2] << 8) | buf[3];
        *az = ((uint16_t) buf[4] << 8) | buf[5];
    }
    return res;;
}

/**
 * I2C连续写函数
 * @param addr 设备地址
 * @param reg 寄存器地址
 * @param len 数据长度
 * @param buf 写入数据
 * @return HAL_Status
 */
uint8_t MPU_Write_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) {
    uint8_t ans = HAL_I2C_Mem_Write(DEF_I2C_HANDLE,(uint16_t)MPU_ADDR, (uint16_t)reg,I2C_MEMADD_SIZE_8BIT,buf,len,0xff);
    return ans;
}

/**
 * I2C连续读
 * @param addr 设备地址
 * @param reg 寄存器地址
 * @param len 数据长度
 * @param buf 读取数据缓冲区
 * @return HAL_Status
 */
uint8_t MPU_Read_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) {
    uint8_t ans = HAL_I2C_Mem_Read(DEF_I2C_HANDLE,(uint16_t)MPU_ADDR,(uint16_t)reg,I2C_MEMADD_SIZE_8BIT,buf,len,0xff);
    return ans;
}

/**
 * I2C单字节写
 * @param reg 寄存器地址
 * @param data 写入数据
 * @return HAL_Status
 */
uint8_t MPU_Write_Byte(uint8_t reg, uint8_t data) {
    uint8_t data_temp[1] = {data};
    uint8_t ans = HAL_I2C_Mem_Write(DEF_I2C_HANDLE,MPU_ADDR,(uint16_t)reg,I2C_MEMADD_SIZE_8BIT,data_temp,1,0xff);
    return ans;
}

/**
 * I2C单字节读
 * @param reg 读取寄存器地址
 * @return HAL_Status
 */
uint8_t MPU_Read_Byte(uint8_t reg) {
    uint8_t read_buf[1] = {0};
    HAL_I2C_Mem_Read(DEF_I2C_HANDLE,MPU_ADDR,(uint16_t)reg,I2C_MEMADD_SIZE_8BIT,read_buf,1,0xff);
    return read_buf[0];
}


